import sensor, image, time
from pyb import Servo
from pyb import UART
import math

white_threshold   = ((88, 100, -20, 8, -11, 52))#13, 49, 18, 61, 6, 47)#(76, 100, -11, 3, -76, 10)，(93, 100, -6, 22, -9, 43)
green_threshold = (29, 97, -36, -14, -13, 26)
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # use RGB565.
sensor.set_framesize(sensor.VGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
#RIO=(52,3,224,220)
clock = time.clock() # Tracks FPS.
uart = UART(3, 9600, timeout_char=1000)                         # i使用给定波特率初始化
uart.init(128000, bits=8, parity=None, stop=1, timeout_char=1000) # 使用给定参数初始化
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob
pan_error = 0
tilt_error = 0
s = 0

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.
    fps = clock.fps()
    blobs = img.find_blobs([white_threshold],x_stride=1,y_stride=1)
    green_blobs = img.find_blobs([white_threshold])
    if blobs:
        max_blob = find_max(blobs)
        pan_error = max_blob.cx()
        tilt_error = max_blob.cy()
        a,b,c,d = max_blob.rect()
       # s = math.sqrt(c*d)
       # img.draw_rectangle([a,b,c,d]) # rect
       # img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy
        buf = bytearray([9,8,int(fps),int(pan_error)-50,int(tilt_error),13,10])
       # buf=bytearray([14])8
    else:
        buf = bytearray([9,8,int(fps),0,0,0,13,10])
    uart.write(buf)
    print([int(fps),(pan_error)-50,(tilt_error),int(s)])
